/***************************************************************************//**
 * @file MSc_Location.h
 * @author  Marek M. Cel <marekcel@mscsim.org>
 *
 * @section LICENSE
 *
 * Copyright (C) 2013 Marek M. Cel
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
 * THE SOFTWARE.
 ******************************************************************************/
#ifndef MSC_LOCATION_H
#define MSC_LOCATION_H

////////////////////////////////////////////////////////////////////////////////

#include "MSc_Matrix3x3.h"
#include "MSc_Vector3.h"

////////////////////////////////////////////////////////////////////////////////

/**
 * This class is used to store and calculate location expressed in WGS axis
 * system and geodetic coordinate system. It also provides functions
 * to calculate rotation matricies between WGS and (North-East-Down) NED axis
 * systems as well as function calculating local normal to ellipsoid vector and
 * gravity acceleration.
 * @see NIMA TR-8350.2 - Department of Defence World Geodetic System 1984
 */
class MSc_Location
{
public:

    /** Constructor. */
    MSc_Location();

    /** Copy constructor. */
    MSc_Location( const MSc_Location &loc );

    /**
     * Returns location object contructed from given geodetic coordinates.
     * @param lat geodetic latitude [rad]
     * @param lon geodetic longitude [rad]
     * @param alt altitude above mean sea level (geoid) [m]
     */
    static MSc_Location getFromGeo( double lat, double lon, double alt );

    /** Returns location object contructed from given WGS coordinates. */
    static MSc_Location getFromWGS( double x, double y, double z );

    /** Returns location object contructed from given WGS coordinates. */
    static MSc_Location getFromWGS( const MSc_Vector3 &vect );

    /**
     * Returns Earth angular velocity vector expressed in WGS axis system.
     * @return [rad/s] Earth angular velocity vector expressed in WGS axis system
     */
    static MSc_Vector3 getOmega_WGS();

    /**
     * Converts geodetic coordinates into WGS coordinates.
     * @param lat geodetic latitude [rad]
     * @param lon geodetic longitude [rad]
     * @param alt altitude above mean sea level (geoid) [m]
     * @param x resulting WGS x-coordinate [m]
     * @param y resulting WGS y-coordinate [m]
     * @param z resulting WGS z-coordinate [m]
     */
    static void geo2wgs( double lat, double lon, double alt, double &x, double &y, double &z );

    /**
     * Converts geodetic coordinates into WGS coordinates.
     * @param lat geodetic latitude [rad]
     * @param lon geodetic longitude [rad]
     * @param alt altitude above mean sea level (geoid) [m]
     * @param vect resulting WGS coordinates vector [m]
     */
    static void geo2wgs( double lat, double lon, double alt, MSc_Vector3 &vect );

    /**
     * Converts WGS coordinates into geodetic coordinates.
     * @param x WGS x-coordinate [m]
     * @param y WGS y-coordinate [m]
     * @param z WGS z-coordinate [m]
     * @param lat resulting geodetic latitude [rad]
     * @param lon resulting geodetic longitude [rad]
     * @param alt resulting altitude above mean sea level (geoid) [m]
     */
    static void wgs2geo( double x, double y, double z, double &lat, double &lon, double &alt );

    /**
     * Converts WGS coordinates into geodetic coordinates.
     * @param vect WGS coordinates vector [m]
     * @param lat resulting geodetic latitude [rad]
     * @param lon resulting geodetic longitude [rad]
     * @param alt resulting altitude above mean sea level (geoid) [m]
     */
    static void wgs2geo( const MSc_Vector3 &vect, double &lat, double &lon, double &alt );

    inline double        getX()         const { return m_loc_WGS.x(); }
    inline double        getY()         const { return m_loc_WGS.y(); }
    inline double        getZ()         const { return m_loc_WGS.z(); }
    inline double        getAlt()       const { return m_alt;         }
    inline double        getLat()       const { return m_lat;         }
    inline double        getLon()       const { return m_lon;         }
    inline MSc_Vector3   getWGS()       const { return m_loc_WGS;     }
    inline MSc_Vector3   getNorm_WGS()  const { return m_norm_WGS;    }
    inline MSc_Vector3   getGrav_WGS()  const { return m_grav_WGS;    }
    inline MSc_Matrix3x3 getT_ned2wgs() const { return m_T_ned2wgs;   }
    inline MSc_Matrix3x3 getT_wgs2ned() const { return m_T_wgs2ned;   }

    /**
     * Updates WGS coordinates due to given geodetic coordinates.
     * @param lat geodetic latitude [rad]
     * @param lon geodetic longitude [rad]
     * @param alt altitude above mean sea level (geoid) [m]
     */
    void setGeo( double lat, double lon, double alt );

    /**
     * Updates geodetic coordinates due to given WGS coordinates.
     * @param x WGS x-coordinates [m]
     * @param y WGS y-coordinates [m]
     * @param z WGS z-coordinates [m]
     */
    void setWGS( double x, double y, double z );

    /**
     * Updates geodetic coordinates due to given WGS coordinates.
     * @see MSc_Location::setWGS(double,double,double)
     * @param vect coordinates expressed in WGS axis system [m]
     */
    void setWGS( const MSc_Vector3 &vect );

private:

    static const double m_a;        ///< [m] major ellipsoid axis
    static const double m_f;        ///< [-] ellipsoid flattening

    static const double m_b;        ///< [m] minor ellipsoid axis
    static const double m_a2;       ///< [m^2] major ellipsiod axis squared
    static const double m_b2;       ///< [m^2] minor ellipsoid axis squared
    static const double m_e2;       ///< [-] ellipsoid first eccentricity squared
    static const double m_e;        ///< [-] ellipsoid first eccentricity
    static const double m_eprim2;   ///< [-] ellipsoid second eccentricity squared
    static const double m_eprim;    ///< [-] ellipsoid second eccentricity

    static const double m_GM;       ///< [m^3/s^2] Earth’s gravitational constant (mass of Earth’s atmosphere included)
    static const double m_omega;    ///< [rad/s] angular velocity of the Earth ( 360deg / 23:56:04.091 )
    static const double m_gamma_e;  ///< [m/s^2] theoretical (normal) gravity at the equator (on the ellipsoid)
    static const double m_gamma_p;  ///< [m/s^2] theoretical (normal) gravity at the pole (on the ellipsoid)
    static const double m_k;        ///< [-] theoretical (normal) gravity formula constant
    static const double m_m;        ///< [-] m = omega^2 * a^2 * b / GM

    double m_lat;   ///< [rad] geodetic latitude
    double m_lon;   ///< [rad] geodetic longitude
    double m_alt;   ///< [m] altitude above mean sea level

    MSc_Vector3 m_loc_WGS;      ///< [m] location coordinates expressen in WGS axis system

    MSc_Vector3 m_norm_WGS;     ///< [-] normal to ellipsoid vector expressed in WGS axis system
    MSc_Vector3 m_grav_WGS;     ///< [m/s^2] gravity acceleration vector expressed in WGS axis system

    MSc_Matrix3x3 m_T_ned2wgs;  ///< matrix of rotation from NED to WGS axis system
    MSc_Matrix3x3 m_T_wgs2ned;  ///< matrix of rotation from WGS to NED axis system

    /**
     * This function updates location (normal to ellipsoid vector, gravity
     * acceleration vector and rotation matricies) due to current WGS coordinates.
     * @see NIMA TR-8350.2 - Department of Defence World Geodetic System 1984, p. 4-2
     */
    void update();
};

////////////////////////////////////////////////////////////////////////////////

#endif // MSC_LOCATION_H
